/**
 * @file data_exchange.cpp
 * @brief 负责与上位机进行通信，接收指令以及回传坐标
 * @author Linfu Wei (ghowoght@qq.com)
 * @version 1.0
 * @date 2021-05-09
 * 
 * @copyright Copyright (c) 2021  WHU-EIS
 * 
 */

#include <iostream>
#include <fstream>
#include <string>
#include <sstream>

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <serial/serial.h>

#include <nav_msgs/Odometry.h>  
#include <tf/transform_broadcaster.h>
#include <data_exchange/tutorialsConfig.h>
#include <dynamic_reconfigure/client.h>
#include <geometry_msgs/Twist.h>

#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/NavSatStatus.h>
#include <visualization_msgs/MarkerArray.h>

#include <thread>
#include <mutex>

#include <math.h>
#include <eigen3/Eigen/StdVector>

#include <globalplanner/BLH2locpos.hpp>
using al_topo_map::BLH2locpos;

#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>

serial::Serial ser;

class gps_st{
public:
	int longitude_i32; // 经度 / deg
	int latitude_i32; // 维度 / deg
	double longitude_f64; // 经度 / deg
	double latitude_f64; // 维度 / deg
};
int position_length = 0;
std::vector<gps_st> gps_goal_array;
std::vector<Eigen::Vector3d> goal_xy_array;
sensor_msgs::NavSatFix current_gps_goal;
int current_goal_idx = 0;
bool get_new_goals = false;
geometry_msgs::Pose current_pose;
int8_t gps_status = 0;

std::mutex m_; // 锁

ros::Publisher gps_goal_pub;

/**
 * @brief 数据类型转换联合体，用于float和字节数组间的转换
 */
typedef union
{
	float data_f32;
	int data_i32;
	unsigned char data8[4];
} data_u;
data_u data_temp;

geometry_msgs::Twist vel_max;



void send_position(serial::Serial& ser, int longitude, int latitude){
	data_u _temp;
	uint8_t data_to_send[100]; // 待发送的字节数组
	int _cnt = 0;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x55;

	data_to_send[_cnt++]=0x15; // data_length

	data_to_send[_cnt++]=0x1e; // 读位置

	// 当前位置
	_temp.data_i32 = longitude;
	data_to_send[_cnt++]=_temp.data8[0];
	data_to_send[_cnt++]=_temp.data8[1];
	data_to_send[_cnt++]=_temp.data8[2];
	data_to_send[_cnt++]=_temp.data8[3]; // 最高位

	_temp.data_i32 = latitude;
	data_to_send[_cnt++]=_temp.data8[0];
	data_to_send[_cnt++]=_temp.data8[1];
	data_to_send[_cnt++]=_temp.data8[2];
	data_to_send[_cnt++]=_temp.data8[3]; // 最高位

	// 下一个点坐标
	if(gps_goal_array.size() != 0){
		longitude = gps_goal_array[gps_goal_array.size() - 1].longitude_i32;
		latitude = gps_goal_array[gps_goal_array.size() - 1].latitude_i32;
	}
	else{
		longitude = 0;
		latitude = 0;
	}
	_temp.data_i32 = longitude;
	data_to_send[_cnt++]=_temp.data8[0];
	data_to_send[_cnt++]=_temp.data8[1];
	data_to_send[_cnt++]=_temp.data8[2];
	data_to_send[_cnt++]=_temp.data8[3]; // 最高位

	_temp.data_i32 = latitude;
	data_to_send[_cnt++]=_temp.data8[0];
	data_to_send[_cnt++]=_temp.data8[1];
	data_to_send[_cnt++]=_temp.data8[2];
	data_to_send[_cnt++]=_temp.data8[3]; // 最高位

	// 当前方向角，北向为零，右手系
	_temp.data_i32 = (int)(tf::getYaw(current_pose.orientation) * 180 / M_PI * 100);
	data_to_send[_cnt++]=_temp.data8[0];
	data_to_send[_cnt++]=_temp.data8[1];
	data_to_send[_cnt++]=_temp.data8[2];
	data_to_send[_cnt++]=_temp.data8[3]; // 最高位

	// ROS_INFO("yaw: %d", _temp.data_i32);

	// GPS状态
	data_to_send[_cnt++]=gps_status;

	char checkout = 0;
    for(int i = 3; i < _cnt; i++)
    {
        checkout += data_to_send[i];
    }
	data_to_send[_cnt++] = checkout;

    // 串口发送
    ser.write(data_to_send, _cnt);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "data_exchange_node");
    ros::NodeHandle nh("~");

	ros::NodeHandle n;

	actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_client_("move_base");
	ROS_INFO("Waiting to connect to move_base server");
    move_base_client_.waitForServer();
    ROS_INFO("Connected to move_base server");

	BLH2locpos blh2locpos;

	gps_st gps;

	std::string path_points_path;
	nh.param("path_points_path", path_points_path, std::string(""));
	ROS_INFO("path_points_path: %s", path_points_path.c_str());

	double min_dist;
	nh.param("min_dist", min_dist, 1.0);
	ROS_INFO("min_dist: %f", min_dist);

	ros::Subscriber gps_sub = n.subscribe<sensor_msgs::NavSatFix>("/rtk_gps", 1, [&](const sensor_msgs::NavSatFix::ConstPtr msgs){
		gps.latitude_f64 = msgs->latitude;
		gps.longitude_f64 = msgs->longitude;
		gps.latitude_i32 = (int)(gps.latitude_f64 * 10000000);
		gps.longitude_i32 = (int)(gps.longitude_f64 * 10000000);

		gps_status = msgs->status.status;

		ROS_INFO("longitude: %d, latitude: %d", gps.longitude_i32, gps.latitude_i32);
	});

	ros::Subscriber odom_sub = n.subscribe<nav_msgs::Odometry>("/odom", 10, [&](const nav_msgs::Odometry::ConstPtr msgs){
		current_pose = msgs->pose.pose;
	});
    
    //订阅服务，并设置回调函数用于回读改变后的最新参数
    // dynamic_reconfigure::Client<data_exchange::tutorialsConfig> client("data_exchange", [&](const data_exchange::tutorialsConfig config){});
    // data_exchange::tutorialsConfig config;

    std::string serial_port;
	int serial_baudrate;
	nh.param("serial_port", serial_port, std::string("/dev/ttyUSB1"));
	nh.param("serial_baudrate", serial_baudrate, 115200);
	// 打开串口
	try
	{
		ser.setPort(serial_port);
		ser.setBaudrate(serial_baudrate);
		serial::Timeout to = serial::Timeout::simpleTimeout(1000);
		ser.setTimeout(to);
		ser.open();
	}
	catch(serial::IOException &e)
	{
		ROS_INFO_STREAM("Failed to open port ");
		return -1;
	}
	ROS_INFO_STREAM("Succeed to open port" );

	// 清空串口缓存区
	ser.read(ser.available());

	ros::Rate loop_rate(30);	

	int state = 0;
	unsigned char data_receive[100] = {0};
	uint8_t data_return[1000] = {0};
	int data_cnt_return = 0;
	int data_length = 0;
	int data_cnt = 0;
	uint8_t mode = 0;
    while(ros::ok())
    {
		// std::string gpgga = "$GPGGA,121252.000,3937.300932,N,11611.60046,E,1,05,2.0,45.9,M,-5.7,M,,0000*77\n";
		// ser.write(gpgga);
		if(ser.available())
		{
			// ROS_INFO("state: %d", state);
            uint8_t buffer = 0;
			size_t bytes_read = ser.read(&buffer, 1);
			
			if(state == 0 && buffer == 0xAA)
			{
				data_return[data_cnt_return++] = buffer;
				state++;
			}
			else if(state == 1 && buffer == 0x55)
			{
				data_return[data_cnt_return++] = buffer;
				state++;
			}
			else if(state == 2)
			{
				data_return[data_cnt_return++] = buffer;
				data_length = buffer - 4;
				// position_length
				// ROS_INFO("%d", data_length);
				state++;
			}
			else if(state == 3){
				data_return[data_cnt_return++] = buffer;
				ROS_INFO("mode: %x", buffer);
				mode = buffer;
				if(mode == 0x09) // 坐标点集
				{
					state = 4;
					data_cnt = 0;
				}
				else if(mode == 0x1e) // 小黑读坐标
				{
					state = 5;
					data_cnt = 0;
				}
				else if(mode == 0x01) // 关机
				{
					state = 6;
					data_cnt = 0;
				}
				else if(mode == 0x02) // 重启
				{
					state = 7;
					data_cnt = 0;
				}
				else if(mode == 0x03) // 暂停
				{
					state = 8;
					data_cnt = 0;
				}
				else if(mode == 0x04) // 设置最大速度
				{
					state = 9;
					data_cnt = 0;
				}
				else if(mode == 0x07) // 手动模式
				{
					state = 10;
					data_cnt = 0;
				}
				else
					state = 0;
			}
			else if(state == 4) // 获取坐标点集
			{
				static int _state = 0;
				if(_state == 0){
					data_return[data_cnt_return++] = buffer;
					position_length = buffer;
					_state ++;
				}
				else if(_state == 1){
					data_return[data_cnt_return++] = buffer;
					data_receive[data_cnt++] = buffer;
					if(data_cnt == data_length)
					{
						// 进行校验
						// uint8_t checkout = position_length;
						int checkout = position_length + mode;;
						for(int k = 0; k < data_length - 1; k++)
						{
							checkout += data_receive[k];
						}
						// ROS_INFO("checkout: %x checkout_origin: %x", (uint8_t)checkout, data_receive[data_cnt - 1]);
						if((uint8_t)checkout == data_receive[data_cnt - 1]) // 串口接收到的最后一个字节是校验码 
						{
							std::lock_guard<std::mutex> lock(m_);
							gps_goal_array.clear();
							goal_xy_array.clear();
							move_base_client_.cancelAllGoals(); // 取消之前发起的服务
							
							for(int k = 0; k < position_length; k++){
								gps_goal_array.push_back(gps_st());
								data_temp.data8[0] = data_receive[8 * k + 0];
								data_temp.data8[1] = data_receive[8 * k + 1];
								data_temp.data8[2] = data_receive[8 * k + 2];
								data_temp.data8[3] = data_receive[8 * k + 3];							
								gps_goal_array[k].longitude_i32 = data_temp.data_i32;
								gps_goal_array[k].longitude_f64 = gps_goal_array[k].longitude_i32 / 10000000.0; // 浮点数

								data_temp.data8[0] = data_receive[8 * k + 4];
								data_temp.data8[1] = data_receive[8 * k + 5];
								data_temp.data8[2] = data_receive[8 * k + 6];
								data_temp.data8[3] = data_receive[8 * k + 7];							
								gps_goal_array[k].latitude_i32 = data_temp.data_i32;
								gps_goal_array[k].latitude_f64 = gps_goal_array[k].latitude_i32 / 10000000.0;
								// ROS_INFO("longitude: %d latitude: %d", gps_goal_array[k].longitude_i32, gps_goal_array[k].latitude_i32); // 浮点数
								ROS_INFO("longitude: %.10f latitude: %.10f", gps_goal_array[k].longitude_f64, gps_goal_array[k].latitude_f64); // 浮点数

							}

							// 将路径点从BLH坐标转换到XY坐标
							for(int i = 0; i <gps_goal_array.size(); i++){
								Eigen::Vector3d temp_blh, temp_xy;
								temp_blh << gps_goal_array[i].latitude_f64 / 180.0 * M_PI, gps_goal_array[i].longitude_f64 / 180.0 * M_PI, 0;
								blh2locpos.BLH2xy(temp_blh, temp_xy);
								goal_xy_array.push_back(temp_xy);
							}

							// 将XY坐标系的路径点写进文件
							std::ofstream outfile(path_points_path);
							for(int i = 0; i < goal_xy_array.size(); i++)
							{
								geometry_msgs::Point p;
								outfile << std::setiosflags(std::ios::fixed) << std::setprecision(8) << goal_xy_array[i].x()  << " " 
										<< std::setiosflags(std::ios::fixed) << std::setprecision(8) << goal_xy_array[i].y() << std::endl;
								// outfile << std::setiosflags(std::ios::fixed) << std::setprecision(8) << gps_goal_array[i].latitude_f64  << " " 
								// 		<< std::setiosflags(std::ios::fixed) << std::setprecision(8) << gps_goal_array[i].longitude_f64 << std::endl;
							}
							outfile.close();

							// 发起导航服务							
							geometry_msgs::Point target_position;
							target_position.x = goal_xy_array[goal_xy_array.size() - 1].x();
							target_position.y = goal_xy_array[goal_xy_array.size() - 1].y();
							target_position.z = goal_xy_array[goal_xy_array.size() - 1].z();
							move_base_msgs::MoveBaseGoal goal;
							goal.target_pose.pose.position = target_position;
							goal.target_pose.pose.orientation.w = 1;
							goal.target_pose.header.frame_id = "map";
							goal.target_pose.header.stamp = ros::Time::now();
							move_base_client_.sendGoal(goal);
							// current_goal_idx = 0;
							// std::thread t([&](){
							// 	while(true){
							// 		loop_rate.sleep();
							// 		std::lock_guard<std::mutex> lock(m_);
							// 		double x_bias = current_pose.position.x - goal_xy_array[current_goal_idx].x();
							// 		double y_bias = current_pose.position.y - goal_xy_array[current_goal_idx].y();
							// 		double dist = sqrt(x_bias * x_bias + y_bias * y_bias);
							// 		if(dist < min_dist) // 如果距离小于设定值，则认为到达了这个点
							// 		{
							// 			current_goal_idx++;
							// 			if(current_goal_idx >= goal_xy_array.size()){
							// 				break;
							// 			}
							// 		}
							// 		// 判断和终点的距离
							// 		x_bias = current_pose.position.x - goal_xy_array[goal_xy_array.size() - 1].x();
							// 		y_bias = current_pose.position.y - goal_xy_array[goal_xy_array.size() - 1].y();
							// 		dist = sqrt(x_bias * x_bias + y_bias * y_bias);
							// 		if(dist < min_dist){
							// 			current_goal_idx = goal_xy_array.size() - 1;
							// 			break;
							// 		}

							// 	}
							// });
							// t.detach();
							// move_base_client_.waitForResult();
						}
						_state = 0;
						state = 0;
						data_cnt = 0;
						ser.write(data_return, data_cnt_return);
						data_cnt_return = 0;
					}
					
				}
			}
			else if(state == 5) // 返回坐标
			{
				if(buffer == mode)
				{
					std::lock_guard<std::mutex> lock(m_);
					send_position(ser, gps.longitude_i32, gps.latitude_i32);
					state = 0;
				}
			}
			else if(state == 6) // 关机
			{
				data_return[data_cnt_return++] = buffer;
				ser.write(data_return, data_cnt_return);
				data_cnt_return = 0;

				if(buffer == mode)
				{
					// send_position(ser, 115200, 9600);
					state = 0;
				}
			}
			else if(state == 7) // 重启
			{
				data_return[data_cnt_return++] = buffer;
				ser.write(data_return, data_cnt_return);
				data_cnt_return = 0;
				if(buffer == mode)
				{
					// send_position(ser, 115200, 9600);
					state = 0;
				}
			}		
			else if(state == 8) // 暂停
			{
				data_return[data_cnt_return++] = buffer;
				ser.write(data_return, data_cnt_return);
				data_cnt_return = 0;
				if(buffer == mode)
				{
					move_base_client_.cancelAllGoals();
					gps_goal_array.clear();
					state = 0;
				}
			}
			else if(state == 9) // 设置最大速度
			{
				data_return[data_cnt_return++] = buffer;
				data_receive[data_cnt++] = buffer;
				if(data_cnt == data_length + 1)
				{
					// 进行校验
					// uint8_t checkout = position_length;
					int checkout = mode;;
					for(int k = 0; k < data_length; k++)
					{
						checkout += data_receive[k];
					}
					// ROS_INFO("checkout: %x checkout_origin: %x", (uint8_t)checkout, data_receive[data_cnt - 1]);
					if((uint8_t)checkout == data_receive[data_cnt - 1]) // 串口接收到的最后一个字节是校验码 
					{
						data_temp.data8[0] = data_receive[0];
						data_temp.data8[1] = data_receive[1];
						data_temp.data8[2] = data_receive[2];
						data_temp.data8[3] = data_receive[3];		
						vel_max.linear.x = data_temp.data_f32;				
						data_temp.data8[0] = data_receive[4];
						data_temp.data8[1] = data_receive[5];
						data_temp.data8[2] = data_receive[6];
						data_temp.data8[3] = data_receive[7];							
						vel_max.angular.z = data_temp.data_f32;	
						ROS_INFO("linear: %f angular: %f", vel_max.linear.x, vel_max.angular.z);

						state = 0;
						data_cnt = 0;
						ser.write(data_return, data_cnt_return);
						data_cnt_return = 0;

					}
					
					
				}
			}
			else if(state == 10) // 手动模式
			{
				data_return[data_cnt_return++] = buffer;
				ser.write(data_return, data_cnt_return);
				data_cnt_return = 0;
				if(buffer == mode)
				{
					move_base_client_.cancelAllGoals();
					gps_goal_array.clear();
					state = 0;
				}
			}

		}
        ros::spinOnce();
        // loop_rate.sleep();
    }


}
